///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/vector/matrix.cpp
///
/// \brief   implementation file for the formic::Matrix class
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#include<cmath>
#include<complex>
#include<stack>
//#include<iostream>

#include<formic/vector/matrix.h>
#include<formic/numeric/numeric.h>

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Computes and returns the exponential of a matrix by evaluating its tailor series until
///        all elements of the next term in the series are smaller than the provided threshold.
///
/// \param[in]      m        the matrix to exponentiate
/// \param[in]      tol      the tolerance below which further terms in the series are ignored
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template<class S, class L>
formic::Matrix<S,L> formic::matrix_exponent(const formic::ConstMatrix<S,L> & m, const double tol) {

  // check tolerance sanity
  if ( tol <= 0.0 )
    throw formic::Exception("tolerance must be positive in formic::matrix_exponent");

  // check for square matrix
  if ( m.rows() != m.cols() )
    throw formic::Exception("matrix must be square in formic::matrix_exponent");

  // initialize the return value to the identity matrix
  formic::Matrix<S,L> r = formic::identity_matrix<S,L>(m.rows());

  // get matrix to hold each tailor series term
  formic::Matrix<S,L> p = r.clone();

  // compute the tailor series of the matrix exponential until
  // the element-wise tolerance is reached
  double x = 1.0;
  for (double max_diff = 2.0 * tol; max_diff > tol; x += 1.0) {

    // get the next term in the exponent tailor series
    p = m * p;
    p *= formic::unity(S()) / x;

    // add the term to the total
    r += p;

    // get the maximum magnitude of the elements of p
    max_diff = -1.0;
    for (int j = 0; j < p.cols(); j++)
    for (int i = 0; i < p.rows(); i++)
      max_diff = std::max(std::abs(p(i,j)), max_diff);

    //std::cout << "matrix exponent iter " << x << "    max_diff = " << max_diff << std::endl;

  }
  //std::cout << std::endl;

  // return the exponentiated matrix
  return r;

}

template formic::Matrix<double,int>
         formic::matrix_exponent(const formic::ConstMatrix<double,int> &,
                                 const double);
template formic::Matrix<std::complex<double>,int>
         formic::matrix_exponent(const formic::ConstMatrix<std::complex<double>,int> &,
                                 const double);
template formic::Matrix<double,size_t>
         formic::matrix_exponent(const formic::ConstMatrix<double,size_t> &,
                                 const double);
template formic::Matrix<std::complex<double>,size_t>
         formic::matrix_exponent(const formic::ConstMatrix<std::complex<double>,size_t> &,
                                 const double);

///////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Computes and returns the derivative adjoint of a matrix for the matrix
///        exponential function given the original matrix and the derivative adjoints 
///        of the exponentiated matrix.
///
/// \param[in]      m        the original (not-exponentiated) matrix
/// \param[in]      a        the derivative adjoints of the exponentiated matrix
/// \param[in]      tol      the tolerance below which further terms in the series are ignored
///
///////////////////////////////////////////////////////////////////////////////////////////////////
template<class S, class L>
formic::Matrix<S,L> formic::matrix_exponent_der_adj(const formic::ConstMatrix<S,L> & m,
                                                    const formic::ConstMatrix<S,L> & a,
                                                    const double tol) {

  //return a.clone();
  //return a + 0.5 * formic::unity(S()) * a * m.t() + 0.5 * formic::unity(S()) * m.t() * a;

  // check tolerance sanity
  if ( tol <= 0.0 )
    throw formic::Exception("tolerance must be positive in formic::matrix_exponent_der_adj");

  // check for square matrices
  if ( m.rows() != m.cols() )
    throw formic::Exception("original matrix must be square in formic::matrix_exponent_der_adj");
  if ( a.rows() != a.cols() )
    throw formic::Exception("exponentiated matrix's derivative adjoint must be square in formic::matrix_exponent_der_adj");

  // check that matrices are the same size
  if ( m.rows() != a.rows() )
    throw formic::Exception("original matrix must be the same size as the exponentiated matrix's derivative adjoint in formic::matrix_exponent_der_adj");

  // get matrix to hold each tailor series term
  formic::Matrix<S,L> p = formic::identity_matrix<S,L>(m.rows());

  // get container to hold tailor series history
  std::stack<formic::ConstMatrix<S,L> > pstack;

  // compute the tailor series terms until
  // the element-wise tolerance is reached
  double x = 1.0;
  for (double max_diff = 2.0 * tol; max_diff > tol; x += 1.0) {

    // save the tailor series term
    pstack.push(p);

    // get the next term in the exponent tailor series
    p = m * p;
    p *= formic::unity(S()) / x;

    // get the maximum magnitude of the elements of p
    max_diff = -1.0;
    for (int j = 0; j < p.cols(); j++)
    for (int i = 0; i < p.rows(); i++)
      max_diff = std::max(std::abs(p(i,j)), max_diff);

  }

  // the matrix p will now hold the adjoint of each tailor series term
  p <<= a;

  // initialize the return value to zero
  formic::Matrix<S,L> r(m.rows(), m.cols(), formic::zero(S()));

  // get the adjoint contribution from each term in the tailor series
  x -= 1.0;
  for ( ; x > 0.5; x -= 1.0) {

    // make sure we didn't mess up
    if ( pstack.empty() )
      throw formic::Exception("unexpectedly empty pstack in formic::matrix_exponent_der_adj");

    // add this tailor series term's contribution to the overall derivative
    r += ( p * pstack.top().t() ) * ( formic::unity(S()) / x );

    // discard the intermediate we no longer need
    pstack.pop();

    // update the accumulation of the intermediates' adjoints
    p = a + ( formic::unity(S()) / x ) * m.t() * p;

  }

  // make sure we didn't mess up
  if ( ! pstack.empty() )
    throw formic::Exception("unexpectedly non-empty pstack in formic::matrix_exponent_der_adj");

  // return the derivative adjoint
  return r;

}

template formic::Matrix<double,int> formic::matrix_exponent_der_adj(const formic::ConstMatrix<double,int> &,
                                                                    const formic::ConstMatrix<double,int> &,
                                                                    const double);
template formic::Matrix<std::complex<double>,int> formic::matrix_exponent_der_adj(const formic::ConstMatrix<std::complex<double>,int> &,
                                                                                  const formic::ConstMatrix<std::complex<double>,int> &,
                                                                                  const double);
template formic::Matrix<double,size_t> formic::matrix_exponent_der_adj(const formic::ConstMatrix<double,size_t> &,
                                                                       const formic::ConstMatrix<double,size_t> &,
                                                                       const double);
template formic::Matrix<std::complex<double>,size_t> formic::matrix_exponent_der_adj(const formic::ConstMatrix<std::complex<double>,size_t> &,
                                                                                     const formic::ConstMatrix<std::complex<double>,size_t> &,
                                                                                     const double);
